psoptim_subparallel<- function (par,  particle, fn, 
                                fullpar=NULL,
                                gr = NULL, 
                                lower = -1, upper = 1,outfile,specification,
                                datamoments,
                                healthprob, marriageprob,foodstamps,numkids,
                                weightmat,Asset,Wage, spWage,seed=NA,agelength=NA,
                                wagenoise,spwagenoise,
                                shocks.wage=NA, shocks.spwage=NA, shocks.health=NA, shocks.allow=NA, shocks.reassess=NA,
                                shocks.jobloss=NA, shocks.spjobloss=NA, shocks.marriage=NA, marriageprob_init, numsims,
                                lumpsum=0,
                                select, selectC, control = list(),
                                onlysingle=0,onlymarried=0,onlyFT=0, workDI=0,noSSDI=0,
                                noflow=FALSE,nocomp=FALSE,
                                noeventDI=TRUE,
                                nostockDI=TRUE,nocontflow=TRUE,
                                ptwork_halfcost=FALSE,
                                single_samecost=FALSE,
                                ptwork_halfdis=FALSE,
                                single_samedis=FALSE,
                                spouse_samejobrisk=FALSE,
                                single_sameallowanceprob=FALSE,
                                work_noallowanceprob=FALSE,
                                single_sameprefs = TRUE,
                                health_simpleprefs = TRUE,
                                nospmoments=FALSE,
                                noworkDImoments=FALSE,
                                consmoments_simple=TRUE,workmoments_simple=TRUE,DImoments_simple=TRUE,
                                ...) {
  
  
  fn1 <- function(par,particle) {
    tryCatch(fn(par,particle=particle,
                outfile=paste0(outfile,particle),specification=specification,datamoments=datamoments,seed=seed,
                healthprob=healthprob, marriageprob=marriageprob, foodstamps=foodstamps,numkids=numkids,
                weightmat=weightmat,Asset=Asset,Wage=Wage, spWage=spWage, agelength=agelength,
                wagenoise=wagenoise,spwagenoise=spwagenoise,
                shocks.wage=shocks.wage, shocks.spwage=shocks.spwage, shocks.health=shocks.health, shocks.allow=shocks.allow, shocks.reassess=shocks.reassess,
                shocks.jobloss=shocks.jobloss, shocks.spjobloss=shocks.spjobloss,
                shocks.marriage=shocks.marriage, marriageprob_init=marriageprob_init, numsims=numsims, lumpsum=lumpsum,
                select=select, selectC=selectC,
                onlysingle=onlysingle,onlymarried=onlymarried,onlyFT=onlyFT,workDI=workDI,noSSDI=noSSDI,
                noflow=noflow,nocomp=nocomp,
                noeventDI=noeventDI, nostockDI=nostockDI, nocontflow = nocontflow,
                ptwork_halfcost=ptwork_halfcost,
                single_samecost=single_samecost,
                ptwork_halfdis=ptwork_halfdis,
                single_samedis=single_samedis,
                spouse_samejobrisk=spouse_samejobrisk,
                single_sameallowanceprob=single_sameallowanceprob,
                work_noallowanceprob=work_noallowanceprob,
                single_sameprefs = single_sameprefs,
                health_simpleprefs = health_simpleprefs,
                nospmoments=nospmoments,
                noworkDImoments=noworkDImoments,
                consmoments_simple=consmoments_simple,
                workmoments_simple=workmoments_simple,
                DImoments_simple=DImoments_simple, ...)/p.fnscale,
             error=function(e) {
               write_csv(x=as.data.frame(t(c(particle,par,Inf,-999))),path=paste0(outfile,particle,".csv"),append=TRUE,col_names=FALSE)
               print(c("RUN ERROR from particle: ",particle, " and params: ",paramvals))
               return(Inf)
             })
  }
  
  mrunif <- function(n, m, lower, upper) {
    return(matrix(runif(n * m, 0, 1), nrow = n, ncol = m) * 
             (upper - lower) + lower)
  }
  norm <- function(x) sqrt(sum(x * x))
  rsphere.unif <- function(n, r) {
    temp <- runif(n)
    return((runif(1, min = 0, max = r)/norm(temp)) * temp)
  }
  svect <- function(a, b, n, k) {
    temp <- rep(a, n)
    temp[k] <- b
    return(temp)
  }
  mrsphere.unif <- function(n, r) {
    m <- length(r)
    temp <- matrix(runif(n * m), n, m)
    return(temp %*% diag(runif(m, min = 0, max = r)/apply(temp, 
                                                          2, norm)))
  }
  npar <- length(par)
  lower <- as.double(rep(lower, , npar))
  upper <- as.double(rep(upper, , npar))
  con <- list(trace = 0, fnscale = 1, maxit = 1000L, maxf = Inf, 
              abstol = -Inf, reltol = 0, REPORT = 10, s = NA, k = 3, 
              p = NA, w = 1/(2 * log(2)), c.p = 0.5 + log(2), c.g = 0.5 + 
                log(2), d = NA, v.max = NA, max.restart = Inf, 
              maxit.stagnate = Inf, vectorize = FALSE, hybrid = FALSE, 
              hybrid.control = NULL, trace.stats = FALSE, type = "SPSO2007")
  nmsC <- names(con)
  con[(namc <- names(control))] <- control
  if (length(noNms <- namc[!namc %in% nmsC])) 
    warning("unknown names in control: ", paste(noNms, collapse = ", "))
  if (any(upper == Inf | lower == -Inf)) 
    stop("fixed bounds must be provided")
  p.type <- pmatch(con[["type"]], c("SPSO2007", "SPSO2011")) - 
    1
  if (is.na(p.type)) 
    stop("type should be one of \"SPSO2007\", \"SPSO2011\"")
  p.trace <- con[["trace"]] > 0L
  p.fnscale <- con[["fnscale"]]
  p.maxit <- con[["maxit"]]
  p.maxf <- con[["maxf"]]
  p.abstol <- con[["abstol"]]
  p.reltol <- con[["reltol"]]
  p.report <- as.integer(con[["REPORT"]])
  p.s <- ifelse(is.na(con[["s"]]), ifelse(p.type == 0, floor(10 + 
                                                               2 * sqrt(npar)), 40), con[["s"]])
  p.p <- ifelse(is.na(con[["p"]]), 1 - (1 - 1/p.s)^con[["k"]], 
                con[["p"]])
  p.w0 <- con[["w"]]
  if (length(p.w0) > 1) {
    p.w1 <- p.w0[2]
    p.w0 <- p.w0[1]
  }
  else {
    p.w1 <- p.w0
  }
  p.c.p <- con[["c.p"]]
  p.c.g <- con[["c.g"]]
  p.d <- ifelse(is.na(con[["d"]]), norm(upper - lower), con[["d"]])
  p.vmax <- con[["v.max"]] * p.d
  p.maxrestart <- con[["max.restart"]]
  p.maxstagnate <- con[["maxit.stagnate"]]
  p.vectorize <- as.logical(con[["vectorize"]])
  if (is.character(con[["hybrid"]])) {
    p.hybrid <- pmatch(con[["hybrid"]], c("off", "on", "improved")) - 
      1
    if (is.na(p.hybrid)) 
      stop("hybrid should be one of \"off\", \"on\", \"improved\"")
  }
  else {
    p.hybrid <- as.integer(as.logical(con[["hybrid"]]))
  }
  p.hcontrol <- con[["hybrid.control"]]
  if ("fnscale" %in% names(p.hcontrol)) 
    p.hcontrol["fnscale"] <- p.hcontrol["fnscale"] * p.fnscale
  else p.hcontrol["fnscale"] <- p.fnscale
  p.trace.stats <- as.logical(con[["trace.stats"]])
  if (p.trace) {
    message("S=", p.s, ", K=", con[["k"]], ", p=", signif(p.p, 
                                                          4), ", w0=", signif(p.w0, 4), ", w1=", signif(p.w1, 
                                                                                                        4), ", c.p=", signif(p.c.p, 4), ", c.g=", signif(p.c.g, 
                                                                                                                                                         4))
    message("v.max=", signif(con[["v.max"]], 4), ", d=", 
            signif(p.d, 4), ", vectorize=", p.vectorize, ", hybrid=", 
            c("off", "on", "improved")[p.hybrid + 1])
    if (p.trace.stats) {
      stats.trace.it <- c()
      stats.trace.error <- c()
      stats.trace.f <- NULL
      stats.trace.x <- NULL
    }
  }
  if (p.reltol != 0) 
    p.reltol <- p.reltol * p.d
  if (p.vectorize) {
    lowerM <- matrix(lower, nrow = npar, ncol = p.s)
    upperM <- matrix(upper, nrow = npar, ncol = p.s)
  }
  X <- mrunif(npar, p.s, lower, upper)
  rownames(X)<-names(par)
  if (!any(is.na(par)) && all(par >= lower) && all(par <= 
                                                   upper)) 
    X[, 1] <- par
  if (p.type == 0) {
    V <- (mrunif(npar, p.s, lower, upper) - X)/2
  }
  else {
    V <- matrix(runif(npar * p.s, min = as.vector(lower - 
                                                    X), max = as.vector(upper - X)), npar, p.s)
    p.c.p2 <- p.c.p/2
    p.c.p3 <- p.c.p/3
    p.c.g3 <- p.c.g/3
    p.c.pg3 <- p.c.p3 + p.c.g3
  }
  if (!is.na(p.vmax)) {
    temp<-rep(NA,p.s)
    temp <- apply(V, 2, norm)
    temp <- pmin.int(temp, p.vmax)/temp
    V <- V %*% diag(temp)
  }
  if(!is.null(fullpar)) X[,1:ncol(fullpar)]<-fullpar
  f.x<-rep(NA,p.s)
  f.p<-rep(NA,p.s)
  # if(exists('cl')){
  #   clusterExport(cl,ls(.GlobalEnv))
  #   ex<- Filter(function(x) is.function(get(x,.GlobalEnv)),ls(.GlobalEnv))
  #   clusterExport(cl,ex)
  # }
  # 
  # f.x<- foreach (i = 1:p.s,.combine = 'cbind') %dopar% {
    f.x[particle]<-fn1(X[,particle],particle=particle)
#  }
  stats.feval <- p.s
  #read in updated values:
  P<-matrix(NA,ncol=p.s,nrow=length(par))
  for(particle2 in 1:p.s){
    guessvals<-try(read.csv(paste0(outfile,particle2,".csv"),header=FALSE),silent=TRUE)
    if(class(guessvals)!="try-error"){
    if(is.vector(guessvals)) guessvals<-t(as.matrix(guessvals))
    guessvals<-apply(guessvals,MARGIN=2, function(x) as.numeric(as.character(x)))
    if(is.vector(guessvals)) guessvals<-t(as.matrix(guessvals))
    particleguess<-guessvals[which(guessvals[,length(par)+3]==999),1:(length(par)+2)]
    if(is.vector(particleguess)) particleguess<-t(as.matrix(particleguess))
    #particleguess<-guessvals[guessvals[,1]==particle2,]
    #if(is.vector(particleguess)) particleguess<-t(as.matrix(particleguess))
    if(nrow(particleguess) > 0) {
      P[,particle2] <- particleguess[which.min(particleguess[,length(par)+2]),2:(length(par)+1)]
      f.p[particle2]<-min(particleguess[,length(par)+2])
    }
    else {
      P[,particle2] <- X[,particle2]
      f.p[particle2] <- Inf
    }
    }
    else {
      P[,particle2] <- X[,particle2]
      f.p[particle2] <- Inf
    }
  }
  P[,particle]<-X[,particle]
  f.p[particle]<-f.x[particle]
#  P <- X
#  f.p <- f.x
  P.improved <- rep(FALSE, p.s)
  i.best <- which.min(f.p)
  error <- f.p[i.best]
  init.links <- TRUE
  if (p.trace && p.report == 1) {
    message("It 1: fitness=", signif(error, 4))
    if (p.trace.stats) {
      stats.trace.it <- c(stats.trace.it, 1)
      stats.trace.error <- c(stats.trace.error, error)
      stats.trace.f <- c(stats.trace.f, list(f.x))
      stats.trace.x <- c(stats.trace.x, list(X))
    }
  }
  stats.iter <- 1
  stats.restart <- 0
  stats.stagnate <- 0
  while (stats.iter < p.maxit ) {
    stats.iter <- stats.iter + 1
    if (p.p != 1 && init.links) {
      links <- matrix(runif(p.s * p.s, 0, 1) <= p.p, p.s, 
                      p.s)
      diag(links) <- TRUE
    }
    if (!p.vectorize) {
        index <- 1:p.s
#      for(i in index){
        for(i in particle){
          if (p.p == 1) 
          j <- i.best
        else j <- which(links[, i])[which.min(f.p[links[, 
                                                        i]])]
        temp <- (p.w0 + (p.w1 - p.w0) * max(stats.iter/p.maxit, 
                                            stats.feval/p.maxf))
        V[, i] <- temp * V[, i]
        if (p.type == 0) {

          V[, i] <- V[, i] + runif(npar, 0, p.c.p) * 
            (P[, i] - X[, i])
          if (i != j) 
            V[, i] <- V[, i] + runif(npar, 0, p.c.g) * 
              (P[, j] - X[, i])
        }
        else {
          if (i != j) 
            temp <- p.c.p3 * P[, i] + p.c.g3 * P[, j] - 
              p.c.pg3 * X[, i]
          else temp <- p.c.p2 * P[, i] - p.c.p2 * X[, 
                                                    i]
          V[, i] <- V[, i] + temp + rsphere.unif(npar, 
                                                 norm(temp))
        }
        if (!is.na(p.vmax)) {
          temp <- norm(V[, i])
          if (temp > p.vmax) 
            V[, i] <- (p.vmax/temp) * V[, i]
        }
        X[, i] <- X[, i] + V[, i]
        temp <- X[, i] < lower
        if (any(temp)) {
          X[temp, i] <- lower[temp]
          V[temp, i] <- 0
        }
        temp <- X[, i] > upper
        if (any(temp)) {
          X[temp, i] <- upper[temp]
          V[temp, i] <- 0
        }
      }

        # if(exists('cl')){
        #   clusterExport(cl,c('X'),envir=environment())
        # }
        
        # f.x<- foreach (i = 1:p.s,.combine = 'cbind') %dopar% {
        f.x[particle]<-fn1(X[,particle],particle=particle)
        #  }
        #stats.feval <- p.s
        #read in updated values:
        # guessvals<-read.csv(paste0(outfile,particle,".csv"),header=FALSE)
        # if(is.vector(guessvals)) guessvals<-t(as.matrix(guessvals))
        # guessvals<-apply(guessvals,MARGIN=2, function(x) as.numeric(as.character(x)))
        # if(is.vector(guessvals)) guessvals<-t(as.matrix(guessvals))
        # guessvals<-guessvals[which(guessvals[,length(par)+3]==999),1:(length(par)+2)]
        # for(particle2 in (1:p.s)){
        #   particleguess<-guessvals[guessvals[,1]==particle2,]
          
        for(particle2 in index){
          guessvals<-try(read.csv(paste0(outfile,particle2,".csv"),header=FALSE),silent=TRUE)
          if(class(guessvals)!="try-error"){
          if(is.vector(guessvals)) guessvals<-t(as.matrix(guessvals))
          guessvals<-apply(guessvals,MARGIN=2, function(x) as.numeric(as.character(x)))
          if(is.vector(guessvals)) guessvals<-t(as.matrix(guessvals))
          particleguess<-guessvals[which(guessvals[,length(par)+3]==999),1:(length(par)+2)]
          if(is.vector(particleguess)) particleguess<-t(as.matrix(particleguess))
          if(nrow(particleguess) > 0) {
            #taking most recent values:
            X[,particle2] <- particleguess[nrow(particleguess),2:(length(par)+1)]
            f.x[particle2]<-particleguess[nrow(particleguess),length(par)+2]
            #taking best values so far:
            P[,particle2] <- particleguess[which.min(particleguess[,length(par)+2]),2:(length(par)+1)]
            f.p[particle2]<-min(particleguess[,length(par)+2])
          }
          else {
            X[,particle2] <- P[,particle2]
            f.x[particle2] <- Inf
          }
          }
          else {
            X[,particle2] <- P[,particle2]
            f.x[particle2] <- Inf
          }
        }

      # for(i in index){
      # ###IF NEW GUESS IS OPTIMAL LOCALLY:
      # if (f.x[i] < f.p[i]) {
      #   P[, i] <- X[, i]
      #   f.p[i] <- f.x[i]
      #   ###IF NEW GUESS IS OPTIMAL GLOBALLY:
      #   if (f.p[i] < f.p[i.best]) {
      #     i.best <- i
      #   }
      # }
      # }
      
    }

    if (p.reltol != 0) {
      d <- X - P[, i.best]
      d <- sqrt(max(colSums(d * d)))
      if (d < p.reltol) {
        X <- mrunif(npar, p.s, lower, upper)
        V <- (mrunif(npar, p.s, lower, upper) - X)/2
        if (!is.na(p.vmax)) {
          temp <- apply(V, 2, norm)
          temp <- pmin.int(temp, p.vmax)/temp
          V <- V %*% diag(temp)
        }
        stats.restart <- stats.restart + 1
        if (p.trace) 
          message("It ", stats.iter, ": restarting")
      }
    }
    init.links <- f.p[i.best] == error
    stats.stagnate <- ifelse(init.links, stats.stagnate + 
                               1, 0)
    error <- f.p[i.best]
    if (p.trace && stats.iter%%p.report == 0) {
      if (p.reltol != 0) 
        message("It ", stats.iter, ": fitness=", signif(error, 
                                                        4), ", swarm diam.=", signif(d, 4))
      else message("It ", stats.iter, ": fitness=", signif(error, 
                                                           4))
      if (p.trace.stats) {
        stats.trace.it <- c(stats.trace.it, stats.iter)
        stats.trace.error <- c(stats.trace.error, error)
        stats.trace.f <- c(stats.trace.f, list(f.x))
        stats.trace.x <- c(stats.trace.x, list(X))
      }
    }
  }
  if (error <= p.abstol) {
    msg <- "Converged"
    msgcode <- 0
  }
  else if (stats.feval >= p.maxf) {
    msg <- "Maximal number of function evaluations reached"
    msgcode <- 1
  }
  else if (stats.iter >= p.maxit) {
    msg <- "Maximal number of iterations reached"
    msgcode <- 2
  }
  else if (stats.restart >= p.maxrestart) {
    msg <- "Maximal number of restarts reached"
    msgcode <- 3
  }
  else {
    msg <- "Maximal number of iterations without improvement reached"
    msgcode <- 4
  }
  if (p.trace) 
    message(msg)
  o <- list(par = P[, i.best], value = f.p[i.best], counts = c(`function` = stats.feval, 
                                                               iteration = stats.iter, restarts = stats.restart), convergence = msgcode, 
            message = msg)
  if (p.trace && p.trace.stats) 
    o <- c(o, list(stats = list(it = stats.trace.it, error = stats.trace.error, 
                                f = stats.trace.f, x = stats.trace.x)))
  return(o)
}

